/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.GenericHID;
import edu.wpi.first.wpilibj.parsing.IInputOutput;
import java.lang.*;

/**
 *
 * @author Jarrod Risley
 */
public class LogitechController extends GenericHID implements IInputOutput {

    double centerValue[] = {0,0,0,0,0,0,0,0};
    // Stick Axes (Note: Mode button switches LeftX betwen Gamepad and Stick
    private static final int m_kAxisLeftX = 1;  // Left -
    private static final int m_kAxisLeftY = 2;  // Up -
    private static final int m_kAxisRightX = 4; // Left -
    private static final int m_kAxisRightY = 5; // Up -
    // Stick Buttons
    private static final int m_kButtonLeftTop = 9;
    private static final int m_kButtonRightTop = 10;
    // Gamepad Axes
    private static final int m_kAxisGameX = 6; // Left -
    private static final int m_kAxisGameY = 2; // Up -            
    // Trigger Buttons (Note actual triggers LT, RT are acutally an Axis)
    private static final int m_kButtonLeftB = 5;
    private static final int m_kButtonRightB = 6;
    private static final int m_kAxisTriggers = 3; //Left Trigger +, Right Trigger -
    // Right Side Buttons
    private static final int m_kButtonA = 1;
    private static final int m_kButtonB = 2;
    private static final int m_kButtonX = 3;
    private static final int m_kButtonY = 4;
    //Central Buttons
    private static final int m_kButtonStart = 8;
    private static final int m_kButtonBack = 7;
    // Pseudo Button (positve axes start at base, negative at 2 * base)
    private static final int m_kPseudoButtonBase = 1000;
    private static final int m_kPseudoButtonLeftTrigger = 2 * m_kPseudoButtonBase + m_kAxisTriggers;
    private static final int m_kPseudoButtonRightTrigger = m_kPseudoButtonBase + m_kAxisTriggers;
    private static final int m_kPseudoButtonGameUp = 2 * m_kPseudoButtonBase + m_kAxisGameY;
    private static final int m_kPseudoButtonGameDown = m_kPseudoButtonBase + m_kAxisGameY;
    private static final int m_kPseudoButtonGameLeft = 2 * m_kPseudoButtonBase + m_kAxisGameX;
    private static final int m_kPseudoButtonGameRight = m_kPseudoButtonBase + m_kAxisGameX;

    private static double digitalScaleFactor = 1;
    
    // Define Enumeration of Valid Axes
    public static class AxisType {

        public final int value;
        public static final LogitechController.AxisType kAxisLeftX = new LogitechController.AxisType(m_kAxisLeftX);
        public static final LogitechController.AxisType kAxisLeftY = new LogitechController.AxisType(m_kAxisLeftY);
        public static final LogitechController.AxisType kAxisRightX = new LogitechController.AxisType(m_kAxisRightX);
        public static final LogitechController.AxisType kAxisRightY = new LogitechController.AxisType(m_kAxisRightY);
        public static final LogitechController.AxisType kAxisGameX = new LogitechController.AxisType(m_kAxisGameX);
        public static final LogitechController.AxisType kAxisGameY = new LogitechController.AxisType(m_kAxisGameY);
        public static final LogitechController.AxisType kAxisTriggers = new LogitechController.AxisType(m_kAxisTriggers);

        private AxisType(int axisNumber) {
            this.value = axisNumber;
        }
    }

    // Define Enumeration of Valid Buttons
    public static class ButtonType {

        public final int value;
        public static final LogitechController.ButtonType kButtonLeftB = new LogitechController.ButtonType(m_kButtonLeftB);
        public static final LogitechController.ButtonType kButtonRightB = new LogitechController.ButtonType(m_kButtonRightB);
        public static final LogitechController.ButtonType kButtonLeftTop = new LogitechController.ButtonType(m_kButtonLeftTop);
        public static final LogitechController.ButtonType kButtonRightTop = new LogitechController.ButtonType(m_kButtonRightTop);
        public static final LogitechController.ButtonType kButtonA = new LogitechController.ButtonType(m_kButtonA);
        public static final LogitechController.ButtonType kButtonB = new LogitechController.ButtonType(m_kButtonB);
        public static final LogitechController.ButtonType kButtonX = new LogitechController.ButtonType(m_kButtonX);
        public static final LogitechController.ButtonType kButtonY = new LogitechController.ButtonType(m_kButtonY);
        public static final LogitechController.ButtonType kButtonStart = new LogitechController.ButtonType(m_kButtonStart);
        public static final LogitechController.ButtonType kButtonBack = new LogitechController.ButtonType(m_kButtonBack);
        public static final LogitechController.ButtonType kButtonLeftTrigger = new LogitechController.ButtonType(m_kPseudoButtonLeftTrigger);
        public static final LogitechController.ButtonType kButtonRightTrigger = new LogitechController.ButtonType(m_kPseudoButtonRightTrigger);
        public static final LogitechController.ButtonType kButtonGameUp = new LogitechController.ButtonType(m_kPseudoButtonGameUp);
        public static final LogitechController.ButtonType kButtonGameDown = new LogitechController.ButtonType(m_kPseudoButtonGameDown);
        public static final LogitechController.ButtonType kButtonGameLeft = new LogitechController.ButtonType(m_kPseudoButtonGameLeft);
        public static final LogitechController.ButtonType kButtonGameRight = new LogitechController.ButtonType(m_kPseudoButtonGameRight);

        private ButtonType(int buttonNumber) {
            this.value = buttonNumber;
        }
    }
    private DriverStation m_ds;
    private final int m_port;

    // Constructor
    public LogitechController(final int port) {
        m_ds = DriverStation.getInstance();
        m_port = port;
    }

    public void recenter(int axisNum) {
        centerValue[axisNum] = 0;
        centerValue[axisNum] = getRawAxis(axisNum);
        System.out.println("Axis " + axisNum + " recenter to " + centerValue[axisNum]);
    }
    
     /**
     * Scaling algorithm that returns a scaled double from the
     * function x' = ax^3 + (1 - a)x, where "a" is a throttle value from 0 to 
     * +1.It is possible to have "a" be altered by input from the controller
     * to allow for multiple sensitivities; if this were to come to pass, 
     * scaleFactor would be changed by the Direction Pad on the Driver 
     * controller.
     * 
     * @return The scaled axis value. Returns 0 if the joystick falls in of
     * the dead zone.
     */
    
    public double scale(double axisData, double scaleFactor) {
        
        double scaledValue = (scaleFactor * (axisData) * (axisData) 
                * (axisData));
        
        if (axisData > 0.3 || axisData < -0.3) {
            
            return scaledValue / digitalScaleFactor;
            
        } else {
            
            return 0;
            
        } // end if-else
        
    } // end scale
    
    // GenericHID Interface Functions
    // Hand is ignored for all functions
    public double getRawAxis(int axisNum) {
        return m_ds.getStickAxis(m_port, axisNum) - centerValue[axisNum];
    }

    public boolean getRawButton(int buttonNum) {
        if (buttonNum < m_kPseudoButtonBase) {
            // normal buttons
            return ((0x1 << (buttonNum - 1)) & m_ds.getStickButtons(m_port)) != 0;
        } else if (buttonNum < 2 * m_kPseudoButtonBase) {
            // positive axis pseudo buttons
            return (getRawAxis(buttonNum - m_kPseudoButtonBase) < -0.5);
        } else {
            // negative axis pseudo buttons
            return (getRawAxis(buttonNum - (2 * m_kPseudoButtonBase)) > 0.5);
        }
    }

    public double getX(GenericHID.Hand hand) {
        return getRawAxis(LogitechController.AxisType.kAxisLeftX.value);
    }

    public double getY(GenericHID.Hand hand) {
        return getRawAxis(LogitechController.AxisType.kAxisLeftY.value);
    }

    public double getZ(GenericHID.Hand hand) {
        return getRawAxis(LogitechController.AxisType.kAxisRightX.value);
    }

    public double getTwist() {
        return getRawAxis(LogitechController.AxisType.kAxisRightY.value);
    }

    public double getThrottle() {
        return getRawAxis(LogitechController.AxisType.kAxisGameX.value);
    }

    public boolean getTrigger(GenericHID.Hand hand) {
        return getRawButton(LogitechController.ButtonType.kButtonLeftB.value);
    }

    public boolean getTop(GenericHID.Hand hand) {
        return getRawButton(LogitechController.ButtonType.kButtonLeftTop.value);
    }

    public boolean getBumper(GenericHID.Hand hand) {
        return getRawButton(LogitechController.ButtonType.kButtonA.value);
    }

    // Logitech Controller Specific Functions
    // Axes
    public double getLeftX() {
        return getRawAxis(LogitechController.AxisType.kAxisLeftX.value);
    }

    public double getLeftY() {
        return getRawAxis(LogitechController.AxisType.kAxisLeftY.value);
    }

    public double getRightX() {
        return getRawAxis(LogitechController.AxisType.kAxisRightX.value);
    }

    public double getRightY() {
        return getRawAxis(LogitechController.AxisType.kAxisRightY.value);
    }

    public double getGameX() {
        return getRawAxis(LogitechController.AxisType.kAxisGameX.value);
    }

    public double getGameY() {
        return getRawAxis(LogitechController.AxisType.kAxisGameY.value);
    }

    public double getTriggers() {
        return getRawAxis(LogitechController.AxisType.kAxisTriggers.value);
    }

    // Buttons
    public boolean getButtonA() {
        return getRawButton(LogitechController.ButtonType.kButtonA.value);
    }

    public boolean getButtonB() {
        return getRawButton(LogitechController.ButtonType.kButtonB.value);
    }

    public boolean getButtonX() {
        return getRawButton(LogitechController.ButtonType.kButtonX.value);
    }

    public boolean getButtonY() {
        return getRawButton(LogitechController.ButtonType.kButtonY.value);
    }

    public boolean getButtonLB() {
        return getRawButton(LogitechController.ButtonType.kButtonLeftB.value);
    }

    public boolean getButtonRB() {
        return getRawButton(LogitechController.ButtonType.kButtonRightB.value);
    }

    public boolean getButtonLeftTop() {
        return getRawButton(LogitechController.ButtonType.kButtonLeftTop.value);
    }

    public boolean getButtonRightTop() {
        return getRawButton(LogitechController.ButtonType.kButtonRightTop.value);
    }

    public boolean getButtonStart() {
        return getRawButton(LogitechController.ButtonType.kButtonStart.value);
    }

    public boolean getButtonBack() {
        return getRawButton(LogitechController.ButtonType.kButtonBack.value);

    }

    // Axis buttons
    public boolean getButtonGameUp() {
        return getRawButton(LogitechController.ButtonType.kButtonGameUp.value);
    }

    public boolean getButtonGameDown() {
        return getRawButton(LogitechController.ButtonType.kButtonGameDown.value);
    }

    public boolean getButtonGameLeft() {
        return getRawButton(LogitechController.ButtonType.kButtonGameLeft.value);
    }

    public boolean getButtonGameRight() {
        return getRawButton(LogitechController.ButtonType.kButtonGameRight.value);
    }

    public boolean getButtonLeftTrigger() {
        return getRawButton(LogitechController.ButtonType.kButtonLeftTrigger.value);
    }

    public boolean getButtonRightTrigger() {
        return getRawButton(LogitechController.ButtonType.kButtonRightTrigger.value);
    }
    
    /**
     * Changes the value of the digital scale factor.
     * 
     * @param value Use the values in the DigitalScale command.
     */
    public static void setDigitalScaleFactor(double value) {
        digitalScaleFactor = value;
    } // end setDigitalScaleFactor
    
} // end LogitechController
